/*倒立摆线性化对比*/
#include "graphics.h"
#include "simucpp.hpp"
using namespace simucpp;
using namespace std;

#define SCENE_HALF_WIDTH  512
#define SCENE_HALF_HEIGHT  220
#define SCENE_WIDTH  (SCENE_HALF_WIDTH*2)
#define SCENE_HEIGHT  (SCENE_HALF_HEIGHT*2)
#define CAR_HALF_WIDTH  20
#define CAR_HALF_HEIGHT  10
#define CAR_ALTITUDE  200
#define ROD_LENGTH  180

struct PendulumState{
    PendulumState() {};
    PendulumState(double x, double theta): x(x), theta(theta) {}
    double x, theta;
};

class Pendulum_Simulator
{
public:
    Pendulum_Simulator() {
        SMGain(minu, &sim1);
        SMIntegrator(mintv1, &sim1);
        SMIntegrator(mintx1, &sim1);
        SMIntegrator(mintv2, &sim1);
        SMIntegrator(mintx2, &sim1);
        SMSum(msuma1, &sim1);
        SMSum(msuma2, &sim1);
        SMOutput(mouttheta, &sim1);
        SMOutput(moutx, &sim1);
        sim1.connect(minu, msuma1);
        sim1.connect(mintx1, msuma1);
        sim1.connect(msuma1, mintv1);
        sim1.connect(mintv1, mintx1);
        sim1.connect(minu, msuma2);
        sim1.connect(mintx1, msuma2);
        sim1.connect(msuma2, mintv2);
        sim1.connect(mintv2, mintx2);
        sim1.connect(mintx1, mouttheta);
        sim1.connect(mintx2, moutx);
        sim1.connect(mintx1, minu);
        // sim1.Set_SimStep(1/double(1<<10));
        sim1.Set_EnablePrint(false);
        msuma1->Set_InputGain(1/(ipM*ipl), 0);
        msuma1->Set_InputGain((ipM+ipm)*9.8/(ipM*ipl), 1);
        msuma2->Set_InputGain(1/ipM, 0);
        msuma2->Set_InputGain(ipm*9.8/ipM, 1);
        minu->Set_Gain(-10);
        mintx1->Set_InitialValue(0.1);
        // moutu->Set_EnablePrint(false);
        // mouttheta->Set_EnablePrint(false);
        moutx->Set_EnablePrint(false);
        sim1.Initialize();
        _simstep = sim1.Get_SimStep();
    };
    PendulumState Simulate(double time) {
        int stepcnt = (int)(time/_simstep/1000.0+0.5);
        for (int i=0; i<stepcnt; ++i)
            sim1.Simulate_OneStep();
        return PendulumState(moutx->Get_OutValue(), mouttheta->Get_OutValue());
    }
    double ipM=0.5, ipm=0.2, ipl=0.3;
private:
    Simulator sim1;
    MGain *minu=nullptr;  // 输入期望角度
    MIntegrator *mintv1=nullptr;  // 小车线性化模型-摆杆角速度
    MIntegrator *mintx1=nullptr;  // 小车线性化模型-摆杆角度
    MIntegrator *mintv2=nullptr;  // 小车线性化模型-速度
    MIntegrator *mintx2=nullptr;  // 小车线性化模型-位移
    MSum *msuma1=nullptr;  // 小车线性化模型-摆杆角加速度
    MSum *msuma2=nullptr;  // 小车线性化模型-小车加速度
    MOutput *mouttheta=nullptr;  // 摆杆角度
    MOutput *moutx=nullptr;  // 小车位移
    double _simstep;
};

void Draw_Pendulum(PendulumState car)
{
   int cx = int(car.x*1e3+0.5) + SCENE_HALF_WIDTH;
   int cy = SCENE_HEIGHT - CAR_ALTITUDE - CAR_HALF_HEIGHT;
   bar(cx-CAR_HALF_WIDTH, cy-CAR_HALF_HEIGHT, cx+CAR_HALF_WIDTH, cy+CAR_HALF_HEIGHT);
   cy -= CAR_HALF_HEIGHT;
   int bx = cx - ROD_LENGTH * sin(car.theta);
   int by = cy - ROD_LENGTH * cos(car.theta);
   line(bx, by, cx, cy);
   return;
}

int main()
{
    Pendulum_Simulator psim1;
    PendulumState car1(0, 0);
    double msdelays = 5;
    initgraph(SCENE_WIDTH, SCENE_HEIGHT);
    setbkcolor(0xFFFFFF);
    setcolor(0);
    setfillcolor(0);
    setrendermode(RENDER_MANUAL);
    for (; is_run(); delay_ms(msdelays)) {
        car1 = psim1.Simulate(msdelays);
        cleardevice();
        Draw_Pendulum(car1);
    }
    return 0;
}
